package behavior;

import java.util.ArrayList;

import Interface.BluetoothNXT;
import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;
import lejos.robotics.navigation.TachoPilot;

/**
 * Responsible for analyzing the current panel using the lineAnalyser,
 * wallAnalyser an BCreader (assuming a known location on the panel)
 */
public class AnalyserController {

	private LineAnalyser lineAnalyser;
	private BarcodeAnalyser barcodeAnalyser;
	private WallAnalyser wallAnalyser;
	private TachoPilot pilot;
	private SlopeDetector slopeDetector;
	private int prevPanel=0;
	private int adjustIndex = 0;
	private int currentAdjustIndex = 0;
	private Motor sensorMotor=Motor.A;
	private UltrasonicSensor sonicSensor;
	private int adjustAngleDistance = 10;

	

	public AnalyserController(TachoPilot pilot, int adjustIndex) {
		lineAnalyser = new LineAnalyser(pilot);
		barcodeAnalyser = new BarcodeAnalyser(pilot);
		wallAnalyser = new WallAnalyser();
		this.pilot = pilot;
		slopeDetector = new SlopeDetector(pilot);
		this.adjustIndex = adjustIndex;
		sonicSensor = new UltrasonicSensor(SensorPort.S2);
//		wallAnalyser.test();
	}
	
	public AnalyserController(TachoPilot pilot, int adjustIndex,BluetoothNXT bt) {
		lineAnalyser = new LineAnalyser(pilot);
		barcodeAnalyser = new BarcodeAnalyser(pilot,bt);
		wallAnalyser = new WallAnalyser();
		this.pilot = pilot;
		slopeDetector = new SlopeDetector(pilot);
		this.adjustIndex = adjustIndex;
		sonicSensor = new UltrasonicSensor(SensorPort.S2);
		
//		wallAnalyser.test();
	}
	
	
	
//	public AnalyserController(TachoPilot pilot,BluetoothNXT bt) {
//		lineAnalyser = new LineAnalyser(pilot);
//		barcodeAnalyser = new BarcodeAnalyser(pilot,bt);
//		wallAnalyser = new WallAnalyser(pilot);
//		this.pilot = pilot;
//		slopeDetector = new SlopeDetector(pilot);
//		wallAnalyser.testMiddle();
//	}

	public int AnalysePanel() {

		int panel = 0;
		sensorMotor.resetTachoCount();
		
		if(prevPanel!=4&&prevPanel!=7)
		{
			panel = barcodeAnalyser.analyse();
		}
		else{
			pilot.reset();
			pilot.travel(80);

		}
			
		if (panel != 0) {
			
			if(wantToAdjust(panel))
			{
				straighten();
				getInMiddle();
			}
			
			executePanel(panel);
			prevPanel=panel;	
			sensorMotor.rotate(-sensorMotor.getTachoCount());
			return panel;
		}
			
		if (panel == 0)
			panel = wallAnalyser.analyse();
		if (panel == 0)
			panel = lineAnalyser.analyse();
		
		if(wantToAdjust(panel))
		{
			straighten();
			getInMiddle();
		}
		
		executePanel(panel);
		
//		if (panel == 1) {
//			panel = slopeDetector.analyse(barcodeAnalyser.frontIsPressed(),barcodeAnalyser.backReleased(),prevPanel);
//		}
		
		prevPanel=panel;		
		sensorMotor.rotate(-sensorMotor.getTachoCount());
		
		return panel;
	}

	private boolean wantToAdjust(int panel) {
		
		if(currentAdjustIndex >= adjustIndex)
		{
			if(panel != 7 && panel != 4 && panel != 2 && panel != 8 &&panel!=1)
			{
				currentAdjustIndex = 0;
				return true;
			}
			
		}
		currentAdjustIndex ++;
		return false;
}

	public void TestBarcodeAnalyser() {
		barcodeAnalyser.testReadings();
		while (true) {
			Button.ENTER.waitForPress();
			int panel = barcodeAnalyser.analyse();
			System.out.println("analysed: " + panel);
		}
	}

	public void TestWallAnalyser() {
		while (true) {
			Button.ENTER.waitForPress();
			int panel = wallAnalyser.analyse();
			System.out.println("analysed: " + panel);
		}
	}

	public void TestLineAnalyser() {
		while (true) {
			Button.ENTER.waitForPress();
			int panel = lineAnalyser.analyse();
			System.out.println("analysed: " + panel);
		}
	}

	public void executePanel(int panel) {
		if (panel == 3)
			pilot.rotate(90);
		if (panel == 6)
			pilot.rotate(-90);
	}
	
	
	public int readValue(){
		ArrayList<Integer> vals = new ArrayList<Integer>();
		for(int i = 0; i < 100; i++){
			vals.add(sonicSensor.getDistance());
		}
		int finalValue = vals.get(0);
		for(int value : vals){
			if (value != 255) finalValue = value;
		}
		return finalValue;
	}
	
	public void straighten(){
		sensorMotor.rotate(90);
		int left=sonicSensor.getDistance();
		sleep(80);
		left = sonicSensor.getDistance();
		sensorMotor.rotate(-180);
		int right = sonicSensor.getDistance();
		sensorMotor.rotate(90);

		if(left<right){
			float angle=getAngle(true);
			pilot.rotate(angle);
		}
		else {
			float angle=getAngle(false);
			pilot.rotate(angle);
		}
	}
	
	
	public void getInMiddle(){
		sonicSensor.getDistance(); //Hurr Ima sonicsensor Durr
		sleep(1000);
		ArrayList<Integer> distances = new ArrayList<Integer>(); 
		//getSonicSensor().getDistance()
		sensorMotor.rotate(90); sleep(80);
		distances.add(readValue());
		sensorMotor.rotate(-90); sleep(80);
		distances.add(readValue());
		sensorMotor.rotate(-90); sleep(80);
		distances.add(readValue());
		sensorMotor.rotate(90);
		
		int left = distances.get(0);
		int right = distances.get(2);
		if(left < 40){
//			float angle = getAngle(left,true);
//			pilot.rotate((float)(angle*180/Math.PI));
			sensorMotor.rotate(90);
			left=readValue();
			sensorMotor.rotate(-90);
			pilot.rotate(90);
			pilot.travel(-(37-left));
			pilot.rotate(-90);
		}
		else if(right<40){
//			float angle = getAngle(right, false);
//			pilot.rotate(angle);
//			pilot.rotate(-(float)(angle*180/Math.PI));
			sensorMotor.rotate(-90);
			right=readValue();
			sensorMotor.rotate(90);
			pilot.rotate(-90);
			pilot.travel(-(37-right));
			pilot.rotate(90);
		}
		else if(left>40 && left <70){
//			float angle = getAngle(left,true);
//			pilot.rotate((float)(angle*180/Math.PI));
			sensorMotor.rotate(90);
			left=readValue();
			sensorMotor.rotate(-90);
			pilot.rotate(-90);
			pilot.travel(-(left-37));
			pilot.rotate(90);
		}


		else if(right>40 && right<70){
//			float angle = getAngle(right, false);
//			pilot.rotate(-(float)(angle*180/Math.PI));
			sensorMotor.rotate(-90);
			right=readValue();
			sensorMotor.rotate(90);
			pilot.rotate(90);
			pilot.travel(-(-37+right));
			pilot.rotate(-90);
		}
		sleep(80);
		int forward = readValue();
		if(forward>40&&forward<70){
			pilot.travel(forward-37);
		} 
		else if(forward<40){
			pilot.travel(-(37-forward));
		}
		
		
	}
	
	private float getAngle(boolean left){
		int sign = left ? 1 : -1;
		LCD.drawString("sign:" + sign, 0, 4);
		sensorMotor.rotate(sign*90);
		int distance1= readValue();
		LCD.drawString("dist1:" + distance1,0,0);
		pilot.travel(adjustAngleDistance);
		int distance2= readValue();
		LCD.drawString("dist2:" + distance2,0,1);
		sensorMotor.rotate(sign*-90);
		pilot.travel(-adjustAngleDistance);
		double v = Math.abs(distance1-distance2);
		float angle = (float) Math.atan(v/adjustAngleDistance); //Nog in radians	
		angle *= 180/Math.PI;
		if(distance1 > distance2){
			angle *= -1;
		}
		return sign*angle; //In graden
	}
	
	private void sleep(int milli)
	{
		try {
			Thread.sleep(milli);
		} catch (InterruptedException e) {
			
		}
	}
}
